/*
Copyright 2011. All rights reserved.
Institute of Measurement and Control Systems
Karlsruhe Institute of Technology, Germany

This file is part of libicp.
Authors: Andreas Geiger

libicp is free software; you can redistribute it and/or modify it under the
terms of the GNU General Public License as published by the Free Software
Foundation; either version 3 of the License, or any later version.

libicp is distributed in the hope that it will be useful, but WITHOUT ANY
WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A
PARTICULAR PURPOSE. See the GNU General Public License for more details.

You should have received a copy of the GNU General Public License along with
libicp; if not, write to the Free Software Foundation, Inc., 51 Franklin
Street, Fifth Floor, Boston, MA 02110-1301, USA 
*/

// Demo program showing how libicp can be used

#include <iostream>
#include "icpPointToPlane.h"
#include "icpPointToPoint.h"
#include <opencv2/opencv.hpp>
#include <Eigen/Dense>

using namespace std;

void gen_complex_data(double* input, double* target, int num = 300);
void gen_sinewave_data(double* input, double* target, int num = 100);
void draw_laser_data(cv::Mat& img, double* laser, int num,
		int pixel_per_m, const cv::Scalar& color);

int main (int argc, char** argv) {

  // define a 3 dim problem with 10000 model points
  // and 10000 template points:
  int32_t dim = 2;
  int32_t num = 300;

  // allocate model and template memory
  double* M = (double*)calloc(2 * num, sizeof(double));
  double* T = (double*)calloc(2 * num, sizeof(double));
  // set model and template points
  cout << endl << "Creating model with " << num << " points ..." << endl;
  gen_sinewave_data(M, T, num);
  //gen_complex_data(M, T, num);

  // start with identity as initial transformation
  // in practice you might want to use some kind of prediction here
  Matrix R = Matrix::eye(2);
  Matrix t(2,1);

  // run point-to-plane ICP (-1 = no outlier threshold)
  cout << endl << "Running ICP (point-to-plane, no outliers)" << endl;
  IcpPointToPlane icp(T, num, dim);
  double residual = icp.fit(M, num, R, t, 2.0);

  // results
  cout << endl << "Transformation results:" << endl;
  cout << "R:" << endl << R << endl << endl;
  cout << "t:" << endl << t << endl << endl;
  cout << "Residual:"<<residual;

  // free memory
  free(M);
  free(T);

  // success
  return 0;
}

void gen_complex_data(double* input, double* target, int num)
{
	if (input == NULL || target == NULL)
		return;

	for (int i = 0; i < num; i++) {
		if (i < num/3) {
			input[i * 2 + 0] = -10.0 + 20.0/(num/3)*i;
			input[i * 2 + 1] = 10.0;
		} else if (i < num*2/3) {
			input[i * 2 + 0] = 10.0;
			input[i * 2 + 1] = 10.0 - 20.0/(num/3)*(i - num/3);
		} else {
			input[i * 2 + 0] = 10.0 - 20.0/(num/3)*(i - num*2/3);
			input[i * 2 + 1] = -10.0;
		}
	}

	Eigen::Isometry2f t(Eigen::Matrix<float, 3, 3>::Identity());
	t.rotate(Eigen::Rotation2D<float>(0.08));
	t.pretranslate(Eigen::Vector2f(0.5, 0.3));
	for (int i = 0; i < num; i++) {
		Eigen::Vector2f pt = t * Eigen::Vector2f(input[i * 2 + 0], input[i * 2 + 1]);
		target[i * 2 + 0] = pt(0);
		target[i * 2 + 1] = pt(1);
	}

	cv::namedWindow("img", cv::WINDOW_NORMAL);
	cv::Mat img(600, 600, CV_8UC3, cv::Scalar(255, 255, 255));
	draw_laser_data(img, input, num, 10, cv::Scalar(255, 0, 0)); 
	draw_laser_data(img, target, num, 10, cv::Scalar(0, 255, 0)); 
	cv::imshow("img", img);
	cv::waitKey(0);
}

void gen_sinewave_data(double* input, double* target, int num)
{
	if (input == NULL || target == NULL)
		return;
	int k = 0;
	for (double x = -2.0; x < 2.0; x+=4.0/num) {
		double y = 2 * sin(x);
		input[k*2+0] = x;
		input[k*2+1] = y;
		k++;
	}

	Eigen::Isometry2f t(Eigen::Matrix<float, 3, 3>::Identity());
	t.rotate(Eigen::Rotation2D<float>(0.1));
	t.pretranslate(Eigen::Vector2f(0.3, 0.7));
	for (int i = 0; i < num; i++) {
		Eigen::Vector2f pt = t * Eigen::Vector2f(input[i * 2 + 0], input[i * 2 + 1]);
		target[i * 2 + 0] = pt(0);
		target[i * 2 + 1] = pt(1);
	}

	cv::namedWindow("img", cv::WINDOW_NORMAL);
	cv::Mat img(600, 600, CV_8UC3, cv::Scalar(255, 255, 255));
	draw_laser_data(img, input, num, 100, cv::Scalar(255, 0, 0)); 
	draw_laser_data(img, target, num, 100, cv::Scalar(0, 255, 0)); 
	cv::imshow("img", img);
	cv::waitKey(0);
}

void draw_laser_data(cv::Mat& img, double* laser, int num,
		int pixel_per_m, const cv::Scalar& color)
{
	Eigen::Matrix3f projection_mat;
	projection_mat << pixel_per_m, 0, img.cols*0.5,
				   0, -pixel_per_m, img.rows*0.5,
				   0, 0, 1;
	Eigen::Isometry2f t(projection_mat);
	for (int i = 0; i < num; i++) {
		Eigen::Vector2f pt = t * Eigen::Vector2f(laser[i * 2 + 0], laser[i * 2 + 1]);
		cv::circle(img, cv::Point(pt(0), pt(1)), 1, color);
	}
}
